#include "User_define.h"
#include "f2808_HRPWM_DAC.h"				// Include file specific to DAC


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
double disob2(double[], double);
double equ(double, double[], double);
double disob2_l(double[], double);
double equ_l(double, double[], double);


 
 
/*
void integral(double *position, double speed){
	*(position) = (*position) + speed*STEP;
}
*//*
void ComplentaryFilter(double *position,double gyro_speed,double acc_angle)
{
  //   static double speed_gyro=0;
//   static double angle_gyro=0;
//   static double speed_gyro_1=0;
      static double position_gyro=0.0;
      static double position_acc=0.0; 

//  speed_gyro_1=gyro_angle+gyro_angle*STEP;
//  speed_gyro=speed_gyro_1-angle_gyro*OMEGA_CUT_OFF;
//  angle_gyro=angle_gyro+speed_gyro*STEP;
//  speed_acc=(acc_angle-angle_acc)*OMEGA_CUT_OFF;
//  angle_acc=angle_acc+speed_acc*STEP;
//  *(position)=angle_gyro+angle_acc;
  
  
 //   position_gyro[0]=position_gyro[1];
    position_gyro+=(gyro_speed-position_gyro*OMEGA_CUT_OFF)*DT_CONTROL;
//	position_acc[0]=position_acc[1];
    position_acc+=(acc_angle-position_acc)*OMEGA_CUT_OFF*DT_CONTROL;
    *(position) = position_gyro+position_acc;
  

}
*/
double nonlinear(double u, double xs[]){
	/* Ms2 = 12.3, lcs2 = 0.35, Is2 = 1.46,Ms1 = 0.5;*/
//	double wcs = 3.0;

/*
	double cos_xs2_2 = cos(xs[2]/2);
 	double cos_xs2 = cos(xs[2]);
  	double sin_xs2_2 = sin(xs[2]/2);
	double sin_xs = sin(xs[2]);
    double tan_xs2 = tan(xs[2]);
	double sin_xs2 = tan_xs2*cos_xs2;
  	double inv_cos_xs2 = 1/cos_xs2;
	static double T_dis = 0.0;
	static double PHAIS[NUMBER_OF_FACTOR] = {0.0, 0.0, 0.0, 0.0};
	static int Ys[NUMBER_OF_FACTOR]  = {0, 0, 0, 0};
*/
// PHAIS[0]=(IS2 + LCS2 * LCS2 * MS2) * log((cos_xs2_2 + sin_xs2_2)/(cos_xs2_2 -  sin_xs2_2))/(LCS2 * MS2) + xs[0];

// PHAIS[1]=xs[1] + ((IS2 + LCS2 * LCS2 *MS2) * inv_cos_xs2 * xs[3])/(LCS2 * MS2);

// PHAIS[2]=tan_xs2*(G + ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*xs[3]*xs[3])/(LCS2*MS2));
// PHAIS[3]=2*G*tan_xs2*tan_xs2*xs[3] + (IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*tan_xs2*tan_xs2*xs[3]*xs[3]*xs[3]/(LCS2*MS2) 
// + inv_cos_xs2*inv_cos_xs2*xs[3]*(G + ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*xs[3]*xs[3])/(LCS2*MS2));

// u=((G*LCS2*LCS2*MS2*MS2*cos_xs2*sin_xs)/(IS2 + LCS2*LCS2*MS2) - LCS2*MS2*sin_xs*xs[3]*xs[3] +
//    ((MS1 + MS2 - (LCS2*LCS2*MS2*MS2*cos_xs2*cos_xs2)/(IS2 + LCS2*LCS2*MS2))*
 //   ((-2*G*G*LCS2*MS2*sin_xs*tan_xs2*tan_xs2)/(IS2 + LCS2*LCS2*MS2) - 6*G*inv_cos_xs2*inv_cos_xs2*tan_xs2*xs[3]*xs[3] -
 //   3*G*tan_xs2*tan_xs2*tan_xs2*xs[3]*xs[3] - (3*(IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*inv_cos_xs2*inv_cos_xs2*tan_xs2*xs[3]*xs[3]*xs[3]*xs[3])/(LCS2*MS2) -
 //   ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*tan_xs2*tan_xs2*tan_xs2*xs[3]*xs[3]*xs[3]*xs[3])/(LCS2*MS2) -
 //   (G*LCS2*MS2*inv_cos_xs2*tan_xs2*(G + ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*xs[3]*xs[3])/(LCS2*MS2)))/(IS2 + LCS2*LCS2*MS2) -
 //   2*inv_cos_xs2*inv_cos_xs2*tan_xs2*xs[3]*xs[3]*(G + ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*xs[3]*xs[3])/(LCS2*MS2)) - /* Fs[0] */WCS*WCS*WCS*WCS*(PHAIS[0] - Ys[0]) - /* Fs[1] */
 //  4*WCS*WCS*WCS*(PHAIS[1] - Ys[1]) - /* Fs[2] */6*WCS*WCS*(PHAIS[2] - Ys[2]) - /* Fs[3] */4*WCS*(PHAIS[3] - Ys[3])))/
 //  ((-2*G*LCS2*MS2*sin_xs*tan_xs2)/(IS2 + LCS2*LCS2*MS2) - 2*inv_cos_xs2*inv_cos_xs2*xs[3]*xs[3] - 3*tan_xs2*tan_xs2*xs[3]*xs[3] -(LCS2*MS2*inv_cos_xs2*(G +
 //   ((IS2 + LCS2*LCS2*MS2)*inv_cos_xs2*xs[3]*xs[3])/(LCS2*MS2)))/(IS2 + LCS2*LCS2*MS2)))*R ;
    double cos_xs2_2 = cos(xs[2]/2);
  	double cos_xs2 = cos(xs[2]);
  	double sin_xs2_2 = sin(xs[2]/2);
    double tan_xs2 = tan(xs[2]);
  	double sin_xs2 = tan_xs2*cos_xs2;
	double inv_cos_xs2 = 1/cos_xs2;
	static double T_dis = 0.0;
	static double PHAIS[NUMBER_OF_FACTOR] = {0.0, 0.0, 0.0, 0.0};
	static int Ys[NUMBER_OF_FACTOR]  = {0, 0, 0, 0};


	PHAIS[0]=0.6891405342624854*(log((cos_xs2_2 + sin_xs2_2)/(cos_xs2_2 - sin_xs2_2))) + xs[0];

	PHAIS[1]=xs[1] + 0.6891405342624854*inv_cos_xs2*xs[3];

	PHAIS[2]=tan_xs2*(G + 0.6891405342624854*inv_cos_xs2*xs[3]*xs[3]);

	PHAIS[3]=19.62*(tan_xs2*tan_xs2)*xs[3] + 0.6891405342624854*inv_cos_xs2*(tan_xs2*tan_xs2)*xs[3]*xs[3]*xs[3] + 
((inv_cos_xs2)*(inv_cos_xs2))*xs[3]*(G + 0.6891405342624854*inv_cos_xs2*xs[3]*xs[3]);

	u = (((- FS1*(PHAIS[0] - Ys[0]) - 
    FS2*(PHAIS[1] - Ys[1]) - FS3*(PHAIS[2] - Ys[2]) - FS4*(PHAIS[3] - Ys[3])) - 279.29310558692174*sin_xs2*(tan_xs2*tan_xs2) - 58.86*((inv_cos_xs2)*(inv_cos_xs2))*tan_xs2*xs[3]*xs[3] - 
        29.43*(tan_xs2*tan_xs2)*tan_xs2*xs[3]*xs[3] - 2.0674216027874563*((inv_cos_xs2)*(inv_cos_xs2))*inv_cos_xs2*tan_xs2*xs[3]*xs[3]*xs[3]*xs[3] - 
        0.6891405342624854*inv_cos_xs2*(tan_xs2*tan_xs2)*tan_xs2*xs[3]*xs[3]*xs[3]*xs[3] - 
        14.235122608915482*inv_cos_xs2*tan_xs2*(G + 0.6891405342624854*inv_cos_xs2*xs[3]*xs[3]) - 
        2*((inv_cos_xs2)*(inv_cos_xs2))*tan_xs2*xs[3]*xs[3]*(G + 0.6891405342624854*inv_cos_xs2*xs[3]*xs[3]))/
    (-28.470245217830964*sin_xs2*tan_xs2 - 2*((inv_cos_xs2)*(inv_cos_xs2))*xs[3]*xs[3] - 3*(tan_xs2*tan_xs2)*xs[3]*xs[3] - 
     1.4510828347518328*inv_cos_xs2*(G + 0.6891405342624854*inv_cos_xs2*xs[3]*xs[3])));


u =(61.28220283138115*cos_xs2*sin_xs2 - 4.305*sin_xs2*xs[3]*xs[3] + 
(12.8 - 6.24691160360664*(cos_xs2*cos_xs2))*u)*R;


	u = (u-T_dis)/ R ;
	T_dis =R* disob2(xs, u);
	return (u );
}
double nonlinear_l(double u, double xl[]){

	static double T_dis = 0.0;
	static double PHAIL[NUMBER_OF_FACTOR] = {0.0, 0.0, 0.0};
	static int YL[NUMBER_OF_FACTOR]  = {0, 0, 0};
  
   PHAIL[0]=xl[1] + (JLA2*xl[3])/(JLA1 + JLA2 + ML*LR*LR);

   PHAIL[1]=(G*ML*LR*sin(xl[0]))/(JLA1 + JLA2 + ML*LR*LR);

   PHAIL[2]=(G*ML*LR*cos(xl[0])*xl[1])/(JLA2 + JLA2 + ML*LR*LR);

   u=((G*JLA2*ML*LR*sin(xl[0]))/(JLA1 + JLA2 + ML*LR*LR) - 
   ((JLA1 + JLA2 + ML*LR*LR)*(JLA1 + JLA2 + ML*LR*LR)*(JLA2 - JLA2*JLA2/(JLA1 + JLA2 + ML*LR*LR))*(1/cos(xl[0]))*
    ((/*-WC*WC*WC*/FL1*(PHAIL[0] - YL[0]) -  FL2 /*3*WC*WC*/*(PHAIL[1] - YL[1]) - FL3 /*3*WC*/*(PHAIL[2] - YL[2]))- 
    (G*G*ML*ML*LR*LR*cos(xl[0])*sin(xl[0]))/(JLA1 + JLA2 + ML*LR*LR)*(JLA1 + JLA2 + ML*LR*LR) + (G*ML*LR*sin(xl[0])*xl[1]*xl[1])/(JLA1 + JLA2 + ML*LR*LR)))/
    (G*JLA2*ML*LR));


	u = (u - T_dis);
	T_dis = disob2_l(xl, u);
	return u;
	}



double disob2(double xs[],double u){
	static double x = 0;
	double k1,k2,k3,k4;
	k1 = STEP * equ(x, xs, u);
	k2 = STEP * equ(x + k1 / 2.0, xs, u);
	k3 = STEP * equ(x + k2 / 2.0, xs, u);
	k4 = STEP * equ(x + k3, xs, u);
	x = x + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; 
	
	return ((K1 * (((MS1 + MS2) * xs[1]) + (LCS2 * MS2 * cos(xs[2]) * xs[3])) + x));
}


double equ(double x,double xs[],double u)
{	
	return (-( K1 * x ) - K1 * K1 * (( MS1 + MS2 ) * xs[1] + LCS2 * MS2 * cos( xs[2] ) * xs[3] ) -( K1 * u ));
}


double disob2_l(double xl[],double u){
	static double x = 0;
	double k1,k2,k3,k4;
	k1 = STEP * equ_l(x, xl, u);
	k2 = STEP * equ_l(x + k1 / 2.0, xl, u);
	k3 = STEP * equ_l(x + k2 / 2.0, xl, u);
	k4 = STEP * equ_l(x + k3, xl, u);
	x = x + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; 
	
	return ( K2 * ( JLA1 * xl[1] + JLA2 * xl[3] ) + x); 
}

double equ_l(double x,double xl[],double u)
{
	return ( - ( K2 * x ) - K2 * K2 * IS2 * (  xl[1] + xl[3] ) - K2 * u );
}


void inputConverterSagittal(Uint16 *DAC2_frac,double *us ){

*(DAC2_frac) = ( Q15_SCALE / 3.3) * (( *(us) * RESIST1 * Vs )/( RESIST2 * (Taus)) + OFFSET2+ ERROR); //Convert for DSP signal Sagittal
}

void inputConverterLateral(Uint16 *DAC1_frac,double *ul ){

*(DAC1_frac) = ( Q15_SCALE / 3.3) * (( *(ul) * RESIST1 * Vl )/( RESIST2 * (Taul)) + OFFSET2 + ERROR); //Convert for DSP signal Sagittal
}



/*** end of file *****************************************************/





